import halcon as ha
import os
from robot_arm import RobotArm
from alg import Alg
from threading import *
from vision_3d import V3D
from camera_link_test import Camera
from util_3d import *

if __name__ == '__main__':

    ca = Camera()
    robot = RobotArm()
    t1 = Thread(target=ca.update_image_loop)
    t1.start()
    print("Input number of image for record\n")
    file_number = input()
    print("This is position of obs\n")
    posture_init = robot.get_end_rokae_type()
    print("当前法兰位姿:", posture_init)
    t_init = rokae_pose_to_T(posture_init)

    trans_list = []
    trans_list.append(xyzrpy_to_T(0.01, 0, 0, 0, 0, 0))
    trans_list.append(xyzrpy_to_T(-0.01, 0, 0, 0, 0, 0))
    trans_list.append(xyzrpy_to_T(0.0, 0.01, 0, 0, 0, 0))
    trans_list.append(xyzrpy_to_T(0.0, -0.01, 0, 0, 0, 0))



    for i in range(4):
        posture = robot.get_end_rokae_type()
        print("第"+ i +'次观测')
        print("当前法兰位姿:", posture)
        gen_obs_t = trans_list@t_init
        robot.movel_to_T(gen_obs_t)


    while True:
        pass
        alpha = 57.295779513
        print("Input file index")
        file_index = input()
        # 确保采集到最新数据
        posture = robot.get_end_rokae_type()
        posture = robot.get_end_rokae_type()

        print("当前法兰位姿:", posture)
        img = ca.get_image()
        file_name = f"{file_index}.dat"
        image_file_name = f"{file_index}.png"
        ha.write_image(img, 'png', 0, image_file_name)

        with open(file_name, 'w') as f:
            if file_index == 'o':
                flag, pose_halcon_type = ca.gen_cam_t_calib(type="halcon")
                if flag == True:
                    text1 = (f"self.cam_teach_t_calib_teach_halcon_type = "
                             f"[{pose_halcon_type[0]}, {pose_halcon_type[1]}, {pose_halcon_type[2]}, {pose_halcon_type[3]}, "
                             f"{pose_halcon_type[4]}, {pose_halcon_type[5]}, {pose_halcon_type[6]}]\n")
                    f.write(text1)
                text2 = (f"self.base_t_end_obs_teach_rokae_type = "
                         f"[{posture[0]}, {posture[1]}, {posture[2]}, {posture[3]}, {posture[4]}, {posture[5]}]\n")
                f.write(text2)
            if file_index == 'p':
                text2 = (f"self.base_t_end_grab_teach_rokae_type = "
                         f"[{posture[0]}, {posture[1]}, {posture[2]}, {posture[3]}, {posture[4]}, {posture[5]}]\n")
                f.write(text2)
            if file_index != 'p' and file_index != 'o':
                text1 = f"r {alpha * posture[3]} {alpha * posture[4]} {alpha * posture[5]}\n"
                text2 = f"t {posture[0]} {posture[1]} {posture[2]}\n"
                f.write('f 2\n')
                f.write(text1)
                f.write(text2)





